#*- coding: utf-8 -*-
import threading
import time
import datetime
import socket
import rospy
import moveit_msgs.msg
from transform import *
from ti5_kinematic import Ti5Kinematic
#from Speed.msg import Speed   # 将订阅str_message话题，自定义消息类型Speed::Speed
from std_msgs.msg import String
import moveit_commander
#import moveit_msgs.msg
from moveit_msgs.msg import *
import numpy
import geometry_msgs.msg
import matplotlib.pyplot as plt
import trajectory_msgs.msg as tm
from move_ori import MoveGroupPythonIntefaceTutorial

Vc=-1 #需要在ROS和socket中传递或使用的变量设为全局变量
connections = []
total_connections = 0

class Server(threading.Thread):
    #def __init__(self):
    def __init__(self, socket, address, id, name, signal):
        threading.Thread.__init__(self)
        self.socket = socket
        self.address = address
        self.id = id
        self.name = name
        self.signal = signal

        #self.pub = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20)
        #self.sub = rospy.Subsrcibe('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20)
        self.arm_kinematic = Ti5Kinematic()
        self.last_position=[0,0,0,0,0,0]
        self.tmp_last_position=[0,0,0,0,0,0]
        self.start_point=[0,0,0,0,0,0]
        self.tmp_start_point=[0,0,0,0,0,0]
        self.n8 = numpy.linspace(1,-1,5)
        self.total_end_joint_position = list()

        self.tutorial = MoveGroupPythonIntefaceTutorial()

        group_name = "armgroup"
        self.group = moveit_commander.MoveGroupCommander(group_name)
    def __str__(self):
        return str(self.id) + " " + str(self.address)

    def add_position(self,li_tot):
        self.last_position=li_tot[0][:]
        #for i in li_tot:
        #    print("li_tot:"+str(li_tot))
        if 1<2:
            if 1<2:
                end_end_end=list()

                for li in li_tot[1:]:
                    end_traj_point_list=list()

                    #print()
                    #li_tmp = list()
                    for i in range(6):
                        #print(str(i)*20)
                        if self.last_position[i]>0 and li[i]>0:
                            per_distance  = abs(self.last_position[i]-li[i])/8
                        elif self.last_position[i]<0 and li[i]<0:
                            per_distance  = abs(self.last_position[i]-li[i])/8
                        else:
                            per_distance  = abs(self.last_position[i])+abs(li[i])
                            per_distance = per_distance/8
                        li_tmp = list()
                        if self.last_position[i]>li[i]:
                            if i==3333:
                                for ii in range(8):
                                    li_tmp.append(self.last_position[i]-(per_distance*tt+abs(1-abs(self.n8[ii])/10)))
                            else:
                                for ii in range(8):
                                    tt=ii+1
                                    if i<30:
                                        li_tmp.append(self.last_position[i]-per_distance*tt)
                                    else:

                                        li_tmp.append(self.last_position[i]-per_distance*8)
                        else:
                            if i==33333:
                                for ii in range(8):
                                    li_tmp.append(self.last_position[i]+(per_distance*tt+abs(1-abs(self.n8[ii])/10)))
                            else:
                                for ii in range(8):
                                    tt=ii+1
                                    if i<30:
                                        li_tmp.append(self.last_position[i]+per_distance*tt)
                                    else:

                                        li_tmp.append(self.last_position[i]+per_distance*8)




                        end_traj_point_list.append(li_tmp)
                    self.last_position=li[:]
                    for iiiii in range(8):
                        l = list()
                        l.append(round(end_traj_point_list[0][iiiii],4))
                        l.append(round(end_traj_point_list[1][iiiii],4))
                        l.append(round(end_traj_point_list[2][iiiii],4))
                        l.append(round(end_traj_point_list[3][iiiii],4))
                        l.append(round(end_traj_point_list[4][iiiii],4))
                        l.append(round(end_traj_point_list[5][iiiii],4))
                        end_end_end.append(l)



        return end_end_end





    def filter_radom_point(self,end_list):
        end_list.insert(0,end_list[0])
        end_list.insert(0,end_list[0])
        end_list.insert(-1,end_list[-1])
        end_list.insert(-1,end_list[-1])
        #for iiiii in end_list:
        #    print(str(iiiii))
        end_list=end_list[:]

        tmp_list =end_list[:]
        le = len(end_list)
        static_num =0

        del_list=list()

        for i in range(2,le-1):
            if static_num>7:
                tmp_list=list()
                print("del random>7!!!!!!!!!!!!!!!!!!!!!"*2)
                break
            for ii in range(6):
                target_point=end_list[i][ii]
                if end_list[i-1][ii]>end_list[i][ii] and end_list[i][ii]<end_list[i+1][ii]:
                    if end_list[i-2][ii]<end_list[i-1][ii] or end_list[i+1][ii]>end_list[i+2][ii]:
                        del_list.append(i)
                        static_num+=1
                        break
                elif end_list[i-1][ii]<end_list[i][ii] and end_list[i][ii]>end_list[i+1][ii]:
                    if end_list[i-2][ii]>end_list[i-1][ii] or end_list[i+1][ii]<end_list[i+2][ii]:
                        del_list.append(i)
                        static_num+=1
                        break
        if del_list:
            for d1 in reversed(del_list):
                try:
                    #del tmp_list[d1-2]
                    break
                except:
                    pass
        if len(tmp_list)>4:
            del tmp_list[0]
            del tmp_list[0]
            del tmp_list[-1]
            del tmp_list[-1]

        #print("end filter radom point")
        return tmp_list





    def InfoCallback(self,msg):#ROS订阅的回调函数
        global Vc
        Vc=msg.carspeed   

    def save_end_joint_position(self,tmp_joint):
        len_position = len(self.total_end_joint_position)>2
        if len_position>2:
            ss =1#1 is ok  2 is no 
            for t11 in range(6):
                if self.total_end_joint_position[len_position-2][t11]>self.total_end_joint_position[len_position-1][t11] and self.total_end_joint_position[len_position-1][t11]>tmp_joint[t11]:
                    ss=1
                elif self.total_end_joint_position[len_position-2][t11]<self.total_end_joint_position[len_position-1][t11] and self.total_end_joint_position[len_position-1][t11]<tmp_joint[t11]:
                    ss=1
                else:
                    ss = 0
                    break
            if ss ==1:
                self.total_end_joint_position.append(tmp_joint)
        
            self.total_end_joint_position.append(tmp)



    def send_info(self,client): #SOCKET的发送函数
        global Vc
        while(1):
            if Vc>=0:
                hvjson=Vc #将vc发送出去
                client.sendall(hvjson.encode("utf-8"))
                time.sleep(1)

    def list_get_min(self,li):
        each_cha = 0
        for i in range(6):
            sta = 0
            if li[i]>0 and self.last_position[i]>0:
                sta=1
            if li[i]<0 and self.last_position[i]<0:
                sta=1
            if sta==1:
                each_cha += abs(self.last_position[i]-li[i])
            elif sta==0:
                each_cha += abs(self.last_position[i])+abs(li[i])
            #print("li:"+str(li[i])+" last_position[i]"+str(self.last_position[i])+"each_cha"+str(each_cha))
            #print(str(each_cha))
        return each_cha

    def change_end_traj(self,restart_num,end_point,end_traj_point_list):
        for i in range(6):
            if self.start_point[i]>0 and end_point[i]>0:
                per_distance  = abs(self.start_point[i]-end_point[i])/10
            elif self.start_point[i]<0 and end_point[i]<0:
                per_distance  = abs(self.start_point[i]-end_point[i])/10
            else:
                per_distance  = abs(self.start_point[i])+abs(end_point[i])
                per_distance = per_distance/10
            li_tmp = list()
            if self.start_point[i]>end_point[i]:
                if i==3333:
                    for ii in range(retart_num,10):
                        li_tmp.append(self.start_point[i]-(per_distance*tt+abs(1-abs(self.n8[ii])/10)))
                else:
                    for ii in range(restart_num,10):
                        tt=ii+1
                        if i<3:
                            end_traj_point_list[i][ii]=self.start_point[i]-per_distance*tt
                            #li_tmp.append(self.start_point[i]-per_distance*tt)
                        else:
                            end_traj_point_list[i][ii]=self.start_point[i]-per_distance*10

                            #li_tmp.append(self.start_point[i]-per_distance*20)
            else:
                if i==33333:
                    for ii in range(restart_num,10):
                        li_tmp.append(self.start_point[i]+(per_distance*tt+abs(1-abs(self.n8[ii])/10)))
                else:
                    for ii in range(restart_num,10):
                        tt=ii+1
                        if i<3:
                            end_traj_point_list[i][ii]=self.start_point[i]+per_distance*tt
                            #li_tmp.append(self.start_point[i]+per_distance*tt)
                        else:
                            end_traj_point_list[i][ii]=self.start_point[i]+per_distance*10
                            #li_tmp.append(self.start_point[i]+per_distance*20)

                                #print(str((1-self.n[ii])**3/20)+"^^^"+str(self.start_point[i]-per_distance*tt))
                                #li_tmp.append(self.start_point[i]+per_distance*tt+(1-self.n[ii])**3/20)


            #end_traj_point_list.append(li_tmp)
        return end_traj_point_list



    def run(self): #SOCKET的接收函数
        global data
        global dd
        pub = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20)
        last_li =[0,0,0,0,0,0]
        while self.signal:

            #try:
            end_point=[0,0,0,0,0,0]

            if 1<2:
                try:
                    info=self.socket.recv(320)
                except:
                    print("Client " + str(self.address) + " has disconnected")
                    self.signal = False
                    connections.remove(self)
                    break

                data=info.decode("utf-8")   #接收到的数据
            proc =0
            if data:
                if data[0]=="R":

                    data = data[1:]
                    li_2 = data.split("_")
                    #li_2 = li_1[0].split(" ")
                    try:
                        li = map(float,li_2)
                        #print("start_rpy: "+str(self.start_point))
                        #print("last_position:"+str(self.last_position))
                        print("tcp_recieved:"+str(li))
                        starttime=datetime.datetime.now()
                        li[0]=li[0]/700
                        li[1]=li[1]/700
                        li[2]=li[2]/700
                        end_point=li
                        print("last_li_3"+str(last_li[3]))
                        print("li_3"+str(li[3]))
                        print("last_li_4"+str(last_li[4]))
                        print("li_4"+str(li[4]))
                        if last_li[3]==li[3] and last_li[4]==li[4] and last_li[5]==li[5]:
                            proc = 1
                            last_li=li
                        else:
                            proc=3
                            last_li=li

                    except:
                        proc =0
                        end_point=list()
                elif data[0]=="J":
                    data = data[1:]
                    li_1=data.split("_")
                    li = map(float,li_1)
                    print("tcp_recieved:"+str(li))
                    starttime=datetime.datetime.now()
                    end_point=list()
                    print("@@@"*12)
                    proc =2

                print("proc:"+str(proc))

                if proc==1:
                    #move_xyz.py
                    #a,b = self.tutorial.go_to_pose_goal2(li)
                    #print(str(b))
                    #point_list,current_state,res_list = self.tutorial.go_to_pose_goal2(li)
                    #point_list1,current_state1,res_list1 = self.tutorial.plan_cartesian_path(li[0],li[1],li[2])
                    point_list1,current_state1,res_list1 = self.tutorial.test(li[0],li[1],li[2])
                    print("res_list"*5)
                    print(str(res_list1))

                    #print("###"+str(joint_goal))
                    if point_list1:

                        display_trajectory = moveit_msgs.msg.DisplayTrajectory()
                        display_trajectory.trajectory_start = current_state1
                        display_trajectory.trajectory.append(point_list1)
                        print(type(display_trajectory))
                        #pub.publish(display_trajectory)

                        if len(res_list1)>1:
                            self.last_position = res_list1[-1].positions

                        send_total=""
                        for i in res_list1:
                            #print(i)

                            end_str_tcp = "T"+str(i.positions[0])+"_"+str(i.positions[1])+"_"+str(i.positions[2])+"_"+str(i.positions[3])+"_"+str(i.positions[4])+"_"+str(i.positions[5])+";"
                            send_total+=end_str_tcp
                            print("tcp-send:"+str(end_str_tcp))

                        i9=self.socket.send(send_total.encode("utf-8"))
                elif proc==3:
                    #move_xyz.py
                    #a,b = self.tutorial.go_to_pose_goal2(li)
                    #print(str(b))
                    point_list,current_state,res_list = self.tutorial.go_to_pose_goal2(li)
                    print("proc 3"*5)
                    #print(str(plan))

                    #print("###"+str(joint_goal))
                    if point_list:

                        display_trajectory = moveit_msgs.msg.DisplayTrajectory()
                        display_trajectory.trajectory_start = current_state
                        display_trajectory.trajectory.append(point_list)
                        print(type(display_trajectory))
                        #pub.publish(display_trajectory)

                        if len(res_list)>1:
                            self.last_position = res_list[-1].positions

                        send_total=""
                        for i in res_list:
                            #print(i)

                            end_str_tcp = "T"+str(i.positions[0])+"_"+str(i.positions[1])+"_"+str(i.positions[2])+"_"+str(i.positions[3])+"_"+str(i.positions[4])+"_"+str(i.positions[5])+";"
                            send_total+=end_str_tcp
                            print("tcp-send:"+str(end_str_tcp))

                        i9=self.socket.send(send_total.encode("utf-8"))


                    


                elif proc ==2:
                    #print("pre_start:"+str(self.start_point))
                    #print("pre_last:"+str(self.last_position))
                    end_traj_point_list=list()
                    #print()
                    for i in range(6): 
                        #print(str(i)*20)
                        if self.last_position[i]>0 and li[i]>0:
                            per_distance  = abs(self.last_position[i]-li[i])/20
                        elif self.last_position[i]<0 and li[i]<0:
                            per_distance  = abs(self.last_position[i]-li[i])/20
                        else:
                            per_distance  = abs(self.last_position[i])+abs(li[i])
                            per_distance = per_distance/20
                        li_tmp = list()
                        if self.last_position[i]>li[i]:
                            if i==3333:
                                for ii in range(20):
                                    li_tmp.append(self.last_position[i]-(per_distance*tt+abs(1-abs(self.n8[ii])/10)))
                            else:
                                for ii in range(20):
                                    tt=ii+1
                                    if i<30:
                                        li_tmp.append(self.last_position[i]-per_distance*tt)
                                    else:

                                        li_tmp.append(self.last_position[i]-per_distance*20)
                        else:    
                            if i==33333:
                                for ii in range(20):
                                    li_tmp.append(self.last_position[i]+(per_distance*tt+abs(1-abs(self.n8[ii])/10)))
                            else:
                                for ii in range(20):
                                    tt=ii+1
                                    if i<30:
                                        li_tmp.append(self.last_position[i]+per_distance*tt)
                                    else:
                                        li_tmp.append(self.last_position[i]+per_distance*20)


                        
                        end_traj_point_list.append(li_tmp)
                    end_end_end=list()
                    for iiiii in range(20):
                        l = list()
                        l.append(round(end_traj_point_list[0][iiiii],4))
                        l.append(round(end_traj_point_list[1][iiiii],4))
                        l.append(round(end_traj_point_list[2][iiiii],4))
                        l.append(round(end_traj_point_list[3][iiiii],4))
                        l.append(round(end_traj_point_list[4][iiiii],4))
                        l.append(round(end_traj_point_list[5][iiiii],4))
                        end_end_end.append(l)

                    #for i in end_end_end:
                    #print(str(i))
                    ep =[0,0,0,0,0,0] 
                    target_pose = self.arm_kinematic.forward_kinematic_v2(end_end_end[-1])
                    #print("target:"+str(target_pose))

                    ep[0]=target_pose.x/700
                    ep[1]=target_pose.y/700
                    ep[2]=target_pose.z/700
                    ep[3]=target_pose.roll
                    ep[4]=target_pose.pitch
                    ep[5]=target_pose.yaw
                    self.start_point= ep


                    print("end_end_end[-1]:"+str(end_end_end[-1]))
                    self.last_position =end_end_end[-1]
                    print("last_position: "+str(self.last_position))

                    if 1<2:
                        dp =DisplayTrajectory()
                        rt = RobotTrajectory()

                        jt=tm.JointTrajectory()
                        for e1 in end_end_end:
                            jtp=tm.JointTrajectoryPoint()
                            jtp.positions = e1
                            jt.points.append(jtp)

                        rt.joint_trajectory=jt
                        dp.trajectory.append(rt)
                        
                        end_str_tcp = "J"+str(self.last_position[0])+"_"+str(self.last_position[1])+"_"+str(self.last_position[2])+"_"+str(self.last_position[3])+"_"+str(self.last_position[4])+"_"+str(self.last_position[5])+";"

                        i9=self.socket.send(end_str_tcp.encode("utf-8"))

                        pub.publish(dp)  #send to rostopic  <<charter>>
                else:
                    print("tcp error pass")






#Wait for new connections
def newConnections(socket):
    while True:
        sock, address = socket.accept()
        global total_connections
        connections.append(Server(sock, address, total_connections, "Name", True))
        connections[len(connections) - 1].start()
        print("New connection at ID " + str(connections[len(connections) - 1]))
        total_connections += 1



if __name__ == '__main__':
    #先开启了ROS

    rospy.init_node('position_pubscriber', anonymous=True)   # ROS节点初始化

    
    #ser=Server()
    host = input("Host: ")
    port = int(input("Port: "))
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.bind((host, port))
    sock.listen(5)
    #Create new thread to wait for connections
    newConnectionsThread = threading.Thread(target = newConnections, args = (sock,))
    newConnectionsThread.start()


    #rospy.Subscriber("str_message", Speed, InfoCallback)   # 创建一个Subscriber，订阅名为/str_message的topic，注册回调函数InfoCallback
 
 #再创建了SOCKET       

    #threads = []
    #t1 = threading.Thread(target=ser.send_info, args=(client,))  #开启线程
    #threads.append(t1)
    #t2 = threading.Thread(target=ser.recv_info, args=(client,))
    #threads.append(t2)
    #for t in threads:
    #    print "1212121"
    #    t.start()
    #    time.sleep(1)


